Method for initial alignment of radar assisted airborne strapdown inertial navigation system

ABSTRACT

The invention provides a method for initial alignment of radar assisted airborne strapdown inertial navigation system. By calculating the slant distance and angular position between the radar and the airborne inertial navigation equipment, a nonlinear measurement equation for the initial alignment of the radar assisted inertial navigation system is obtained. The unscented Kalman filter algorithm is used to estimate and compensate the error amount of strapdown inertial navigation system to complete the initial alignment task. The significance of the present invention is to provide an in-flight initial alignment solution when the global positioning system is limited, which has fast convergence speed and high estimation accuracy and has high engineering application value.

TECHNICAL FIELD

The invention relates to an alignment method, in particular to a methodfor initial alignment of radar assisted airborne strapdown inertialnavigation system.

BACKGROUND TECHNOLOGY

The initial alignment technology of strapdown inertial navigation systemis the key technology of inertial navigation, which directly affects theaccuracy of inertial navigation. The current research on aerialalignment is mostly focused on the alignment of shipborne weapons, butthe airborne strapdown inertial navigation system also needs to berestarted in special operations. At present, the commonly used in-flightalignment method is Global Positioning System (GPS) assisted alignment.Considering that GPS is susceptible to blockade restrictions andunavailability in wartime, it is particularly important to find otherauxiliary methods to achieve SINS initial alignment under specialcircumstances. Its ranging range can reach thousands to tens ofthousands of kilometers, the precision of angle measurement is high, andit has the characteristics of continuous tracking, high precisionmeasurement and high data rate output. The setting is simple, which canbe provided to the onboard navigation system via wireless transmission.At present, there is not much research on the initial alignment ofradar-assisted inertial navigation equipment. Usually, the positioninformation in the earth coordinate system output by the radar isdirectly used to construct the quantity measurement. In fact, this modelis inaccurate. As the distance between the target and the radarincreases, its linearized position error will become larger, that is,the measurement noise will also change. This is very unfavorable for thestate estimation and usually causes a large alignment error.

SUMMARY OF THE INVENTION

The purpose of the present invention is to provide a method for initialalignment of radar assisted airborne strapdown inertial navigationsystem, which is a method of initial alignment in airborne strapdowninertial navigation system assisted with slant distance and angularposition information measurement by radar.

The purpose of the present invention is achieved as follows:

Step 1: position and track the target aircraft through the trackingradar configured on the ship;

Step 2: tracking radar measures the slant distance, angular position ofthe target aircraft and the position of the radar will be provided tothe airborne inertial navigation system through wireless transmission;

Step 3: construct the initial alignment state equation of strapdowninertial navigation system;

Step 4: construct a nonlinear measurement equation for strapdowninertial navigation system initial alignment;

Step 5: use the unscented Kalman filter to estimate and compensate theinertial navigation error.

The invention also includes such structural features:

1. Step 3 specifically includes:

A state X is selected as:X=[ϕ^(T)(δv ^(n))^(T)(δP)^(T)(ε^(b))^(T)(∇^(b))^(T)]^(T)

Among them: n is the navigation coordinate system, which coincides withthe local geographic coordinate system, and its x, y, and z axes pointto east, north, and vertical, respectively; b is the carrier coordinatesystem, and its x, y, and z point to the right, front and top of thecarrier, respectively; δP=[δL δλδh]^(T) is the position error vector, δLis the latitude position error, δλ is the longitude position error, δhheight position error; δv^(n)=[δv_(E) δv_(N) δv_(v)]^(T) is the speederror vector, δv_(E) is the eastward velocity error, δv_(N) is thenorthward velocity error, δv_(u) is the celestial velocity error;ϕ=[ϕ_(e) ϕ_(n) ϕ_(u)]^(T) is the platform misalignment angle vector,φ_(e), Φ_(n) and Φ_(u) are the platform misalignment angles in the east,north, and vertical directions, respectively, ε^(b)=[ε_(x) ε_(y)ε_(z)]^(T) is the constant drift vector of the gyro, ε_(x), ε_(y) andε_(z) are the constant drifts of the gyro in the x, y, and z axes,respectively, ∇^(b)=[∇_(x) ∇_(y) ∇_(z)]^(T) is the accelerometerconstant bias vector, and ∇_(x), ∇_(y) and ∇_(z) are the accelerometerconstant bias in the x, y, and z axes;

According to the selected state parameters, the initial alignment stateequation of the inertial navigation system is

$\quad\left\{ \begin{matrix}{\overset{.}{\phi} = {{{- \omega_{in}^{n}} \times \phi} + {M_{12}\delta\; v^{n}} + {M_{13}\delta\; P} - {C_{b}^{n}\left( {ɛ^{b} + ɛ_{w}^{b}} \right)}}} \\{{\delta\;{\overset{.}{v}}^{n}} = {{\left( {C_{b}^{n}f^{b}} \right) \times \phi} + {\left( {{v^{n} \times M_{12}} - \left\lbrack {\left( {{2\omega_{ie}^{n}} + \omega_{en}^{n}} \right) \times} \right\rbrack} \right)\delta\; v^{n}} + {M_{23}\delta\; P} + {C_{b}^{n}\left( {\nabla^{b}{+ \nabla_{w}^{b}}} \right)}}} \\{{\delta\;\overset{.}{P}} = {{M_{32}\delta\; v^{n}} + {M_{33}\delta\; P}}} \\{{\overset{.}{ɛ}}^{b} = 0} \\{{\overset{.}{\nabla}}^{b}{= 0}}\end{matrix} \right.$

Among them: ω_(in) ^(n)=ω_(ie) ^(n)+ω_(en) ^(n) is the projection ofrotation angular velocity of the navigation system relative to theinertial system in the navigation system, which includes two vectorparts: ω_(ie) ^(n) is the projection vector of the earth's rotationangular velocity in the navigation coordinate system, ω_(en) ^(n) is theprojection vector of the rotation angular velocity of the navigationsystem relative to the inertial system caused by the motion of thecarrier on the surface of the earth in the navigation coordinate system.

${\omega_{ie}^{n} = \begin{bmatrix}0 & {\omega_{ie}\cos\; L} & {\omega_{ie}\sin\; L}\end{bmatrix}^{T}},{\omega_{en}^{n} = \left\lbrack {\frac{- v_{N}}{R_{h}}\ \frac{v_{E}}{R_{h}}\ \frac{v_{E}\tan\; L}{R_{h}}} \right\rbrack^{T}}$and ω_(ie) the earth's rotation angular rate scalar, L is the locallatitude, R_(h) is the distance between the carrier and the center ofthe earth, where R_(h)=R_(e)+h, R_(e) is the radius of the earth, h isthe altitude of the carrier, v^(n)=[v_(E) v_(N) v_(u)]^(T) is theprojection of the carrier velocity vector in the navigation coordinatesystem. v_(E), v_(N), and v_(U) are the east velocity, north velocity,and vertical velocity, respectively. f^(b) is the specific force vectorof the accelerometer output carrier coordinate system,

$\mspace{20mu}{{M_{12} = \begin{bmatrix}0 & {{- 1}/R_{h}} & 0 \\{1/R_{h}} & 0 & 0 \\{\tan\;{L/R_{h}}} & 0 & 0\end{bmatrix}},\mspace{20mu}{M_{13} = {\begin{bmatrix}0 & 0 & 0 \\{{- \omega_{ie}}\sin\; L} & 0 & 0 \\{\omega_{ie}\cos\; L} & 0 & 0\end{bmatrix} + \begin{bmatrix}0 & 0 & {v_{N}/R_{h}^{2}} \\0 & 0 & {{- v_{E}}/R_{h}^{2}} \\{v_{E}\sec^{2}{L/R_{h}}} & 0 & {{- v_{E}}\;\tan\;{L/R_{h}^{2}}}\end{bmatrix}}},{M_{23} = {\left( {v^{n} \times} \right)\left( {{2 \cdot \begin{bmatrix}0 & 0 & 0 \\{{- \omega_{ie}}\;\sin\; L} & 0 & 0 \\{\omega_{ie}\;\cos\; L} & 0 & 0\end{bmatrix}} + \begin{bmatrix}0 & 0 & {{- v_{N}}/R_{h}^{2}} \\0 & 0 & {{- v_{E}}/R_{h}^{2}} \\{v_{E}\sec^{2}{L/R_{h}}} & 0 & {{- v_{E}}\;\tan\;{L/R_{h}^{2}}}\end{bmatrix}} \right)}},\mspace{20mu}{M_{32} = \begin{bmatrix}0 & {1/R_{h}} & 0 \\{\sec\;{L/R_{h}}} & 0 & 0 \\0 & 0 & 1\end{bmatrix}},\mspace{20mu}{M_{33} = {\begin{bmatrix}0 & 0 & {{- v_{N}}/R_{h}^{2}} \\{v_{E}\;\sec\; L\;\tan\;{L/R_{h}}} & 0 & {{- v_{E}}\;\sec{L/R_{h}^{2}}} \\0 & 0 & 0\end{bmatrix}\mspace{14mu}{and}}}}\mspace{14mu}$$\mspace{20mu}{C_{b}^{n} = \begin{bmatrix}T_{11} & T_{12} & T_{13} \\T_{21} & T_{22} & T_{23} \\T_{31} & T_{32} & T_{33}\end{bmatrix}}$are the attitude matrix of the sub-inertial navigation, T₁₁, T₁₂, T₁₃,T₂₁, T₂₂, T₂₃, T₃₁, T₃₂ and T₃₃ are the elements of the attitude matrix,ε_(w) ^(b)=[ε_(wx) ^(b) ε_(wy) ^(b) ε_(wz) ^(b)]^(T) the Gaussian whitenoise vector measured by the gyro, ε_(wx) ^(b), ε_(wy) ^(b) and ε_(wz)^(b) are the x, y and z axis gyro measurement Gaussian white noise,∇_(w) ^(b)=[∇_(wx) ^(b) ∇_(wy) ^(b) ∇_(wz) ^(b)]^(T) is the whiteGaussian vector of accelerometer measurement, and ∇_(wx) ^(b), ∇_(wy)^(b) and ∇_(wz) ^(b) are the white Gaussian of x, y and z axialacceleration measurement.

2. Step 4 includes:

Measurement Z=[R β α]^(T) includes slant distance R, azimuth angle β andpitch angle α;

Among them:

${Z = {\begin{bmatrix}R \\\beta \\\alpha\end{bmatrix} = \begin{bmatrix}\sqrt{\left( {dx^{n}} \right)^{2} + \left( {dy^{n}} \right)^{2} + \left( {dz}^{n} \right)^{2}} \\{\arctan\frac{{dz}^{n}}{\sqrt{\left( {dx}^{n} \right)^{2} + \left( {dy}^{n} \right)^{2}}}} \\{{arc}\;\tan\frac{{dx}^{n}}{{dy}^{n}}}\end{bmatrix}}},{\begin{bmatrix}{dx}^{n} & {dy}^{n} & {dz}^{n}\end{bmatrix}^{T} = {{C_{p_{o}}^{n}\begin{bmatrix}{dx}^{e} & {dy}^{e} & {dz}^{e}\end{bmatrix}}^{T}\mspace{14mu}{{and}\begin{bmatrix}{dx}^{n} & {dy}^{n} & {dz}^{n}\end{bmatrix}}T}}$are the relative position vector of the target and the radar in thenavigation coordinate system, C_(P) _(o) ^(n) is the coordinateconversion matrix between the earth's rectangular coordinate system andthe navigation coordinate system. The coordinate of the Earth'srectangular coordinate system is

${\begin{bmatrix}{dx^{e}} \\{dy^{e}} \\{dz^{e}}\end{bmatrix} = {\begin{bmatrix}{\left( {R_{e} + h_{p}} \right){\cos\left( L_{p} \right)}{\cos\left( \lambda_{p} \right)}} \\{\left( {R_{e} + h_{p}} \right){\cos\left( L_{p} \right)}{\sin\left( \lambda_{p} \right)}} \\{\left( {R_{e} + h_{p}} \right){\sin\left( L_{p} \right)}}\end{bmatrix} - \begin{bmatrix}x_{p_{o}}^{e} \\y_{p_{o}}^{e} \\z_{p_{o}}^{e}\end{bmatrix}}},$e represents the Earth's rectangular coordinate system. L_(p)=L_(p)^(s)−δL is the true latitude, λ_(p)=λ_(p) ^(s)−δλ is true longitude,h_(p)=h_(p) ^(s)−δh is true altitude, and L_(p) ^(s), λ_(p) ^(s) andh_(p) ^(s) are the position resolved by the inertial navigation system.

Then the measurement equation for the initial alignment of theradar-assisted strapdown inertial navigation system is:

$Z = {{H\left( {{\delta L},{\delta\lambda},{\delta h}} \right)} + \begin{bmatrix}\omega_{R} \\\omega_{\beta} \\\omega_{\alpha}\end{bmatrix}}$

Among them: ω_(R), ω_(α) and ω_(β) top are white noises that conform tothe zero-mean Gaussian distribution, and the expression of the nonlinearfunction H can be obtained by the above substitution.

Compared with the prior art, the beneficial effects of the presentinvention are: the present invention is based on the slant distance andangular position provided by the radar, considering the transferrelationship between the positioning error of the strapdown inertialnavigation system and the slant distance and angular position, thepresent invention proposes a new alignment scheme for the measurementmodel, which uses the slant distance and angular position as measurementinformation to achieve alignment. First, the present invention providesa new solution for the initial alignment of the airborne inertialnavigation system when the global positioning system is blocked, and hashigh engineering application value. Second, compared with thetraditional radar-assisted inertial navigation system initial alignmentscheme, the advantages of the present invention are reflected in thefact that the position coordinates after the radar measurementparameters are linearized are not selected as the measurement, whichavoids the problem of the statistical characteristics of the measurementnoise changing with the distance. The invention directly uses the slantdistance and angular position information obtained by radar measurementas the quantity measurement, and makes full use of the originalmeasurement information. Since the statistical characteristics of themeasurement noise meet the requirements of the optimal estimation, theKalman filter method can be used to estimate the optimal status. At thesame time, compared with the existing alignment scheme, the proposedscheme can complete the high-precision initial alignment task in alarger distance range.

BRIEF DESCRIPTION OF THE FIGURES

FIG. 1 is a schematic diagram of the present invention;

FIG. 2 is a description of the radar target measurement parameters ofthe present invention;

FIG. 3a is a comparison diagram of horizontal attitude error between thepresent invention and the existing scheme under the condition 1 insimulation experiment, FIG. 3b is a comparison diagram of azimuthattitude error between the present invention and the existing schemeunder the condition 1 in simulation experiment.

FIG. 4a is a comparison diagram of horizontal attitude error between thepresent invention and the existing scheme under the condition 2 insimulation experiment, FIG. 4b is a comparison diagram of azimuthattitude error between the present invention and the existing schemeunder the condition 2 in simulation experiment.

DETAILED DESCRIPTION OF THE INVENTION

The present invention will be further described in detail below withreference to the drawings and specific embodiments.

As shown in FIG. 1, it is a schematic diagram of an initial alignmentsolution for radar-assisted strapdown inertial navigation systemprovided by the present invention. The measurement noise statisticalcharacteristics vary with the slant distance. The invention has theadvantages of high alignment accuracy and short alignment time. Itincludes the following steps:

Step 1: the tracking radar is configured on the ship to locate and trackthe target aircraft. As shown in FIG. 2, P_(o) is the location of theradar carrier, and the polar coordinates P (R, α, β) of any target P inthe air are measured with it as the origin. R is called the slantdistance, which is the linear distance from the radar to the target. αrepresents the azimuth angle, α represents the azimuth angle, which isthe angle between PoB projected on the horizontal plane between theradar and the target line PoP and the true north direction on thehorizontal plane. β represents the elevation angle, which is the anglebetween the PoB projected on the horizontal plane between the radar andthe target line PoP and the lead vertical plane. It is also called theinclination angle or the height angle.

Step 2: tracking radar measures the slant distance, angular position ofthe target aircraft and the location of the radar carrier will beprovided to the airborne inertial navigation system through wirelesstransmission. The location P_(o) of the radar carrier is provided by ahigh-precision inertial navigation device, and its positioning accuracyis high, so the influence of its position error can be ignored. Becauseof its high positioning accuracy, its position error can be ignored.

Step 3: construct the initial alignment state parameters and stateequations of the strapdown inertial navigation system. The statequantity is selected as:X=[ϕ^(T)(δv ^(n))^(T)(δP)^(T)(ε^(b))^(T)(∇^(b))^(T)]^(T)

n is the navigation coordinate system, which coincides with the localgeographic coordinate system, and its x, y, and z axes point to east,north, and vertical, respectively; b is the carrier coordinate system,and its x, y, and z point to the right, front and top of carrier; δP=[δLδλ δh]^(T) is the position error vector, δL is the latitude positionerror, δλ is the longitude position error, δh height position error;δv^(n)=[δv_(E) δv_(N) δv_(U)]^(T) is the speed error vector, δv_(E) isthe eastward velocity error, δv_(N) is the northward velocity error,δv_(U) is the celestial velocity error; ϕ=[ϕ_(e) ϕ_(n) ϕ_(u)]^(T) is theplatform misalignment angle vector, Φ_(e), Φ_(n) and Φ_(u) are theplatform misalignment angles in the east, north, and verticaldirections, respectively, ε^(b)=[ε_(x) ε_(y) ε_(z)]^(T) is the constantdrift vector of the gyro, ε_(x), ε_(y) and ε_(z) are the constant driftsof the gyro in the x, y, and z axes, respectively, ∇^(b)=[∇_(x) ∇_(y)∇_(Z)]^(T) is the accelerometer constant bias vector, and ∇_(x),∇_(y),∇_(z) is the accelerometer constant bias in the x, y, and z axes,and T represents transpose.

Further, according to the selected state parameters can be obtainedinertial navigation system initial alignment state equation is:

$\quad\left\{ \begin{matrix}{\overset{.}{\phi} = {{{- \omega_{in}^{n}} \times \phi} + {M_{12}\delta\; v^{n}} + {M_{13}\delta\; P} - {C_{b}^{n}\left( {ɛ^{b} + ɛ_{w}^{b}} \right)}}} \\{{\delta\;{\overset{.}{v}}^{n}} = {{\left( {C_{b}^{n}f^{b}} \right) \times \phi} + {\left( {{v^{n} \times M_{12}} - \left\lbrack {\left( {{2\omega_{ie}^{n}} + \omega_{em}^{n}} \right) \times} \right\rbrack} \right)\delta\; v^{n}} + {M_{23}\delta\; P} + {C_{b}^{n}\left( {\nabla^{b}{+ \nabla_{w}^{b}}} \right)}}} \\{{\delta\;\overset{.}{P}} = {{M_{32}\delta\; v^{n}} + {M_{33}\delta\; P}}} \\{{\overset{.}{ɛ}}^{b} = 0} \\{{\overset{.}{\nabla}}^{b}{= 0}}\end{matrix} \right.$

Among them, the point on the state quantity represents the firstderivative .ω_(in) ^(n)=ω_(ie) ^(n)+ω_(en) ^(n) is the projection ofrotation angular velocity of the navigation system relative to theinertial system in the navigation system, which includes two vectorparts: ω_(ie) ^(n) is the projection vector of the earth's rotationangular velocity in the navigation coordinate system, ω_(en) ^(n) is theprojection vector of the rotation angular velocity of the navigationsystem relative to the inertial system caused by the motion of thecarrier on the surface of the earth in the navigation coordinate system.

${\omega_{ie}^{n} = \begin{bmatrix}0 & {\omega_{ie}\cos\; L} & {\omega_{ie}\sin\; L}\end{bmatrix}^{T}},{\omega_{en}^{n} = \left\lbrack {\frac{- v_{N}}{R_{h}}\ \frac{v_{E}}{R_{h}}\ \frac{v_{E}\tan\; L}{R_{h}}} \right\rbrack^{T}}$and ω_(ie) are the earth's rotation angular rate scalar, L is the locallatitude, R_(h) is the distance between the carrier and the center ofthe earth, where R_(h)=R_(e)+h, R_(e) is the radius of the earth, h isthe altitude of the carrier, v_(n)=[v_(E) v_(N) v_(U)]^(T) is theprojection of the carrier velocity vector in the navigation coordinatesystem. v_(E), v_(N), and v_(U) are the east velocity, north velocity,and vertical velocity, respectively. f^(b) is the specific force vectorof the accelerometer output carrier coordinate system,

$\mspace{20mu}{{M_{12} = \begin{bmatrix}0 & {{- 1}/R_{h}} & 0 \\{1/R_{h}} & 0 & 0 \\{\tan\;{L/R_{h}}} & 0 & 0\end{bmatrix}},\mspace{20mu}{M_{13} = {\begin{bmatrix}0 & 0 & 0 \\{{- \omega_{ie}}\;\sin\; L} & 0 & 0 \\{\omega_{ie}\cos\; L} & 0 & 0\end{bmatrix} + \begin{bmatrix}0 & 0 & {v_{N}/R_{h}^{2}} \\0 & 0 & {{- v_{E}}/R_{h}^{2}} \\{v_{E}\sec^{2}{L/R_{h}}} & 0 & {{- v_{E}}\tan{L/R_{h}^{2}}}\end{bmatrix}}},{M_{23} = {\left( {v^{n} \times} \right)\left( {{2 \cdot \begin{bmatrix}0 & 0 & 0 \\{{- \omega_{ie}}\sin\; L} & 0 & 0 \\{\omega_{ie}\cos\; L} & 0 & 0\end{bmatrix}} + \ \begin{bmatrix}0 & 0 & {v_{N}/R_{h}^{2}} \\0 & 0 & {{- v_{E}}/R_{h}^{2}} \\{v_{E}\sec^{2}{L/R_{h}}} & 0 & {{- v_{E}}\tan\;{L/R_{h}^{2}}}\end{bmatrix}} \right)}},\mspace{20mu}{M_{32} = \begin{bmatrix}0 & {1/R_{h}} & 0 \\{\sec\;{L/R_{h}}} & 0 & 0 \\0 & 0 & 1\end{bmatrix}},\mspace{20mu}{M_{33} = {\begin{bmatrix}0 & 0 & {{- v_{N}}/R_{h}^{2}} \\{v_{E}\sec\; L\;\tan\;{L/R_{h}}} & 0 & {{- v_{E}}\sec\;{L/R_{h}^{2}}} \\0 & 0 & 0\end{bmatrix}\mspace{14mu}{and}}}}\mspace{14mu}$$\mspace{20mu}{C_{b}^{n} = \begin{bmatrix}T_{11} & T_{12} & T_{13} \\T_{21} & T_{22} & T_{23} \\T_{31} & T_{32} & T_{33}\end{bmatrix}}$are the attitude matrix of the sub-inertial navigation, T₁₁, T₁₂, T₁₃,T₂₁, T₂₂, T₂₃, T₃₁, T₃₂ and T₃₃ are the elements of the attitude matrix,ε_(w) ^(b)=[ε_(wx) ^(b) ε_(wy) ^(b) ε_(wz) ^(b)]^(T) is the Gaussianwhite noise vector measured by the gyro, ε_(wx) ^(b), ε_(wy) ^(b) andε_(wz) ^(b) are the x, y and z axis gyro measurement Gaussian whitenoise, ∇_(w) ^(b)=[∇_(wx) ^(b) ∇_(wy) ^(b) ∇_(wz) ^(b)]^(T) is the whiteGaussian vector of accelerometer measurement, and ∇_(wx) ^(b), ∇_(wy)^(b) and ∇_(wz) ^(b) are the white Gaussian of x, y and z axialacceleration measurement.

Step 4: select the initial alignment measurement parameters of thestrapdown inertial navigation system and construct a nonlinearmeasurement equation. In the existing initial alignment method of theradar-assisted strapdown inertial navigation system, the position erroris selected as the measurement. In the present invention, the slantdistance and the angular position are used as the measurement; then themeasurement: Z=[R β α]^(T) is respectively slant distance R, azimuthangle β and pitch angle α.

Among them:

${Z = {\begin{bmatrix}R \\\beta \\\alpha\end{bmatrix} = \begin{bmatrix}\sqrt{\left( {dx^{n}} \right)^{2} + \left( {dy^{n}} \right)^{2} + \left( {dz}^{n} \right)^{2}} \\{\arctan\frac{{dz}^{n}}{\sqrt{\left( {dx}^{n} \right)^{2} + \left( {dy}^{n} \right)^{2}}}} \\{{arc}\;\tan\frac{{dx}^{n}}{{dy}^{n}}}\end{bmatrix}}},{\begin{bmatrix}{dx}^{n} & {dy}^{n} & {dz}^{n}\end{bmatrix}^{T} = {C_{p_{o}}^{n}\begin{bmatrix}{dx}^{e} & {dy}^{e} & {dz}^{e}\end{bmatrix}}^{T}}$and [dx^(n) dy^(n) dz^(n)]^(T) are the relative position vector of thetarget and the radar in the navigation coordinate system, C_(p) _(o)^(n) is the coordinate conversion matrix between the earth's rectangularcoordinate system and the navigation coordinate system. The coordinateof the earth's rectangular coordinate system is

${\begin{bmatrix}{dx^{e}} \\{dy^{e}} \\{dz^{e}}\end{bmatrix} = {\begin{bmatrix}{\left( {R_{e} + h_{p}} \right)\cos\;\left( L_{p} \right)\cos\;\left( \lambda_{p} \right)} \\{\left( {R_{e} + h_{p}} \right)\cos\;\left( L_{p} \right)\sin\;\left( \lambda_{p} \right)} \\{\left( {R_{e} + h_{p}} \right)\sin\;\left( L_{p} \right)}\end{bmatrix} - \begin{bmatrix}x_{p_{o}}^{e} \\y_{p_{o}}^{e} \\z_{p_{o}}^{e}\end{bmatrix}}},$e represents the earth's rectangular coordinate system. L_(p)=L_(p)^(s)−δL is the true latitude, λ_(p)=λ_(p) ^(s)−δλ is true longitude,h_(p)=h_(p) ^(s)−δh is true altitude, and L_(p) ^(s), λ_(p) ^(s) andh_(p) ^(s) are the position resolved by the inertial navigation system.Then the measurement equation for the initial alignment of theradar-assisted strapdown inertial navigation system is:

$Z = {{H\left( {{\delta L},{\delta\lambda},{\delta\; h}} \right)} + {\begin{bmatrix}\omega_{R} \\\omega_{\beta} \\\omega_{\alpha}\end{bmatrix}.}}$

${Z = {{H\left( {{\delta L},{\delta\lambda},{\delta h}} \right)} + \begin{bmatrix}\omega_{R} \\\omega_{\beta} \\\omega_{\alpha}\end{bmatrix}}},$ω_(R), ω_(α) and ω_(β) are white noises that conform to a zero-meanGaussian distribution, and the expression of the nonlinear function Hcan be obtained by the above substitution.

Step 5: use the unscented Kalman filter to estimate and compensate thestrapdown inertial navigation system error.

The system equations and measurement equations for the initial alignmentof the radar-assisted strapdown inertial navigation system are given insteps 3 and 4. The initial alignment task can be completed only byestimating and compensating the state quantities. Because themeasurement equation is non-linear, this scheme uses the unscentedKalman filter algorithm for state estimation.

(1) Select the initial filter value{circumflex over (X)} ₀ =EX ₀P ₀ =E[(X ₀ −{circumflex over (X)} ₀)][(X ₀ −{circumflex over (X)}₀)^(T)]

System dimension n=15

The weights are:

${W_{0}^{(m)} = \frac{\lambda}{n + \lambda}},\mspace{11mu}{W_{0}^{(c)} = {\frac{\lambda}{n + \lambda} + 1 - a^{2} + b}}\;,\mspace{14mu}{W_{i}^{(m)} = {W_{i}^{(c)} = \frac{\lambda}{2\left( {n + \lambda} \right)}}}$     i = 1, 2, …  , 2n

γ=√{square root over (n+λ)}, λ=α²(n+κ)−n a is a very small positivenumber, 10⁻⁴≤α≤1, κ=3-n, b=2 can be selected.

(2) Calculate 2n+1 σ samples when k−1 (k=1, 2, 3, . . . ){tilde over (χ)}_(k-1) ⁽⁰⁾ ={circumflex over (X)} _(k-1){tilde over (χ)}_(k-1) ^((i)) ={circumflex over (X)} _(k-1)+γ(√{squareroot over (P _(k-1))})_((i)) i=1, 2, . . . , n{tilde over (χ)}_(k-1) ^((i)) ={circumflex over (X)} _(k-1)−γ(√{squareroot over (P _(k-1))})_((i-n)) i=n+1, n+2, . . . , 2n

(3) A predictive model for computing k time

${\chi_{{k/k} - 1}^{*{(i)}} = {{{f\left\lbrack {{\overset{\sim}{\chi}}_{k - 1}^{(i)},u_{k - 1}} \right\rbrack}\mspace{31mu} i} = 0}},1,2,\ldots\mspace{14mu},{2n}$${\hat{X}}_{{k/k} - 1} = {\sum\limits_{i = 0}^{2n}{W_{i}^{(m)}\chi_{{k/k} - 1}^{*{(i)}}}}$$P_{{k/k} - 1} = {{\sum\limits_{i = 0}^{2n}{{W_{i}^{(c)}\left\lbrack {\chi_{{k/k} - 1}^{*{(i)}} - {\hat{X}}_{{k/k} - 1}} \right\rbrack}\;\left\lbrack {\chi_{{k/k} - 1}^{*{(i)}} - {\hat{X}}_{{k/k} - 1}} \right\rbrack}^{T}} + Q_{k - 1}}$

(4) Calculate the one-step prediction sample point at the time of k)χ_(k/k-1) ⁽⁰⁾ ={circumflex over (X)} _(k/k-1)χ_(k/k-1) ^((i)) ={circumflex over (X)} _(k/k-1)+γ(√{square root over (P_(k/k-1))})_((i)) i=1,2, . . . ,nχ_(k/k-1) ^((i)) ={circumflex over (X)} _(k/k-1)−γ(√{square root over (P_(k/k-1))})_((i-n)) i=n+1,n+2, . . . ,2n

(5) Calculation P_((XZ)k/k-1), P_((ZZ)k/k-1)

Z_(k/k − 1)^((i)) = h[χ_(k/k − 1)^((i))],  i = 0, 1, 2, …  , 2n${\overset{\hat{}}{Z}}_{{k/k} - 1} = {{\sum\limits_{i = 0}^{2n}{W_{i}^{(m)}Z_{{k/k} - 1}^{(i)}P_{{({XZ})}_{{k/k} - 1}}}} = {\sum\limits_{i = 0}^{2n}{{W_{i}^{(c)}\left\lbrack {\chi_{{k/k} - 1}^{(i)} - {\overset{\hat{}}{X}}_{{k/k} - 1}} \right\rbrack}\left\lbrack {Z_{{k/k} - 1}^{(i)} - {\overset{\hat{}}{Z}}_{{k/k} - 1}} \right\rbrack}^{T}}}$$P_{{({ZZ})}_{{k/k} - 1}} = {{\sum\limits_{i = 0}^{2n}{{W_{i}^{(c)}\left\lbrack {Z_{{k/k} - 1}^{(i)} - {\overset{\hat{}}{Z}}_{{k/k} - 1}} \right\rbrack}\left\lbrack {Z_{{k/k} - 1}^{(i)} - {\overset{\hat{}}{Z}}_{{k/k} - 1}} \right\rbrack}^{T}} + R_{k}}$

(6) Calculate the gain matrix

K_(k) = P_((XZ)_(k/k − 1))P_((ZZ)_(k/k − 1))⁻¹

(7) Calculate the filter value

${{\overset{\hat{}}{X}}_{k} = {{\overset{\hat{}}{X}}_{{k/k} - 1} + {K_{k}\left\lbrack {Z_{k} - {\overset{\hat{}}{Z}}_{{k/k} - 1}} \right\rbrack}}}{P_{k} = {P_{{k/k} - 1} - {K_{k}P_{{({ZZ})}_{{k/k} - 1}}K_{k}^{T}}}}$

(8) Through the above process, the navigation error of the strapdowninertial navigation system can be estimated, so as to performclosed-loop correction and complete the initial alignment.

The technical solution of the present invention is simulated andverified in combination with specific values below:

Simulation conditions: the initial position error of the inertialnavigation device is set to 10√{square root over (3m)} the parameters ofthe inertial measurement unit as follows: the constant drift of the gyrois 0.01° h, The random drift is 0.001°/√{square root over (h)}, theaccelerometer constant bias is 3×10⁻⁴ g, and the random drift is 5×10⁻⁵g√{square root over (s)}, the sampling time interval is 10 ms; the slantdistance error measured by radar is 10m (1σ), the pitch angle error is0.1° (1σ), and the azimuth angle error is 0.3° (1σ).

Because the ship's position and attitude are provided by the combinationof ship inertial navigation equipment and auxiliary equipment, itsposition error and attitude error can be ignored. To simplify thesimulation complexity, in order to simplify the complexity ofsimulation, considering that the ship is still and the “true north”provided by the ship is error-free. The height of the radar is 5 m, theupdate period is 1 s, the filter filtering period is 1s, the filter isclosed-loop corrected, the simulation time is set to 300 s; the flightspeed is 80 m/s, and the flight height is 1000 m.

In order to verify the effectiveness of the present invention, thesimulation environment is set: the slant distance of simulationcondition 1 is less than 10 km, and the slant distance of simulationcondition 2 is more than 50 km. Compared with the existingradar-assisted alignment scheme. FIG. 3a and FIG. 3b are the attitudeerror comparison diagrams of the present invention and the existingscheme for 200 times Monte Carlo simulation results under conditions 1in simulation experiment. FIG. 4a and FIG. 4b are the attitude errorcomparison diagrams of the present invention and the existing scheme for200 times Monte Carlo simulation results under conditions 2 insimulation experiment. Among them, the thin black solid line is the meancurve of the traditional measurement model scheme, and the thin blackdashed line is the 3σ curve of the traditional measurement model scheme.Among them, the thick black solid line is the mean curve of the newmeasurement model scheme, and the thick black dotted line is the 3σcurve of the new measurement model scheme; Φ_(e), Φ_(n) and Φ_(u) arethe pitch error angle, the roll error angle, and the heading errorangle, respectively.

In summary, the present invention provides a method for initialalignment of radar assisted airborne strapdown inertial navigationsystem. By calculating the slant distance and angular position betweenthe radar and the airborne inertial navigation equipment, a nonlinearmeasurement equation for the initial alignment of the radar assistedinertial navigation system is obtained. The unscented Kalman filteralgorithm is used to estimate and compensate the error amount ofstrapdown inertial navigation system to complete the initial alignmenttask. The significance of the present invention is to provide anin-flight initial alignment solution when the global positioning systemis limited, which has fast convergence speed and high estimationaccuracy and has high engineering application value.

The invention claimed is:
 1. A method for an initial alignment of aradar assisted airborne strapdown inertial navigation system ischaracterized by the following steps: step 1: position and track atarget aircraft through a tracking radar configured on a ship; step 2:the tracking radar measures a slant distance, an angular position of thetarget aircraft and a position of the tracking radar which are thenprovided to the radar assisted airborne strapdown inertial navigationsystem through wireless transmission; step 3: construct an initialalignment state equation of the radar assisted airborne strapdowninertial navigation system; step 4: construct a nonlinear measurementequation for the initial alignment of the radar assisted airbornestrapdown inertial navigation system; step 5: use an unscented Kalmanfilter to estimate and compensate an inertial navigation error.
 2. Themethod for the initial alignment of the radar assisted airbornestrapdown inertial navigation system according to claim 1, wherein thestep 3 specifically includes: a state X is selected as:X=[ϕ^(T)(δv ^(n))^(T)(δP)^(T)(ε^(b))^(T)(∇^(b))^(T)]^(T), where: n is anavigation coordinate system, which coincides with a local geographiccoordinate system, and its x, y, and z axes point to east, north, andvertical, respectively; b is a carrier coordinate system, and its x, y,and z point to right, front and top of a carrier; δP=[δL δλ δh]^(T) is aposition error vector, δL is a latitude position error, δλ a is alongitude position error, δh is a height position error; δv^(n)=[δv_(E)δv_(N) δv_(U)]^(T) is a speed error vector, δv_(E) is an eastwardvelocity error, δv_(N) is a northward velocity error, δv_(U) is acelestial velocity error; ϕ=[ϕ_(e) ϕ_(n) ϕ_(u)]^(T) is a platformmisalignment angle vector, ϕ_(e), ϕ_(n) and ϕ_(u) are platformmisalignment angles in the east, north, and vertical directions,respectively, ε^(b)=[ε_(x) ε_(y) ε_(z)]^(T) is a constant drift vectorof a gyro, ε_(x), ε_(y) and ε_(z) are constant drifts of the gyro in thex, y, and z axes, respectively, ∇^(b)=[∇_(x) ∇_(y) ∇_(z)]^(T) is anaccelerometer constant bias vector, and ∇_(x), ∇_(y), ∇_(z) areaccelerometer constant biases in the x, y, and z axes respectively;according to the selected state X, the initial alignment state equationof the radar assisted airborne strapdown inertial navigation system is$\left\{ {\begin{matrix}{\overset{.}{\phi} = {{{- \omega_{i\; n}^{n}} \times \phi} + {M_{12}\delta\; v^{n}} + {M_{13}\delta\; P} - {C_{b}^{n}\left( {ɛ^{b} + ɛ_{w}^{b}} \right)}}} \\\begin{matrix}{{\delta\;{\overset{.}{v}}^{n}} = {{\left( {C_{b}^{n}f^{b}} \right) \times \phi} + {\left( {{v^{n} \times M_{12}} - \left\lbrack {\left( {{2\;\omega_{ie}^{n}} + \omega_{en}^{n}} \right) \times} \right\rbrack} \right)\delta\; v^{n}} +}} \\{{M_{23}\delta\; P} + {C_{b}^{n}\left( {\nabla^{b}{+ \nabla_{w}^{b}}} \right)}}\end{matrix} \\{{\delta\;\overset{.}{P}} = {{M_{32}\delta\; v^{n}} + {M_{33}\delta\; P}}} \\{{\overset{.}{ɛ}}^{b} = 0} \\{{\overset{.}{\nabla}}^{b}{= 0}}\end{matrix}\quad} \right.$ where: ω_(in) ^(n)=ω_(ie) ^(n)+ω_(en) ^(n)is a projection of a rotation angular velocity of the navigationcoordinate system relative to an inertial system on the navigationcoordinate system, which includes two vector parts: ω_(ie) ^(n) is aprojection vector of an earth's rotation angular velocity in thenavigation coordinate system, ω_(en) ^(n) is a projection vector of arotation angular velocity of the navigation coordinate system relativeto the inertial system caused by a movement of the carrier on a surfaceof the earth in the navigation coordinate system,${\omega_{ie}^{n} = \begin{bmatrix}0 & {\omega_{ie}\cos\; L} & {\omega_{ie}\sin\; L}\end{bmatrix}^{T}},{\omega_{en}^{n} = \begin{bmatrix}\frac{- v_{N}}{R_{h}} & \frac{v_{E}}{R_{h}} & \frac{v_{E}\tan\; L}{R_{h}}\end{bmatrix}^{T}}$ and ω_(ie) is an earth's rotation angular ratescalar, L is a local latitude, R_(h) is a distance between the carrierand a center of the earth, where R_(h)=R_(e)+h, R_(e) is a radius of theearth, h is an altitude of the carrier, v^(n)=[v_(E) v_(N) v_(U)]^(T) isa projection of a carrier velocity vector in the navigation coordinatesystem, v_(E), v_(N), and v_(U) are an east velocity, a north velocity,and a vertical velocity, respectively, f^(b) is a specific force vectorof an accelerometer output carrier coordinate system,$\mspace{20mu}{{M_{12} = \begin{bmatrix}0 & {{- 1}/R_{h}} & 0 \\{1/R_{h}} & 0 & 0 \\{\tan\;{L/R_{h}}} & 0 & 0\end{bmatrix}},\mspace{20mu}{M_{13} = {\begin{bmatrix}0 & 0 & 0 \\{{- \omega_{ie}}\sin\; L} & 0 & 0 \\{\omega_{ie}\cos\; L} & 0 & 0\end{bmatrix} + \begin{bmatrix}0 & 0 & {v_{N}/R_{h}^{2}} \\0 & 0 & {{- v_{E}}/R_{h}^{2}} \\{v_{E}\;\sec^{2}{L/R_{h}}} & 0 & {{- v_{E}}\tan\;{L/R_{h}^{2}}}\end{bmatrix}}},{M_{23} = {\left( {v^{n} \times} \right)\left( {{2 \cdot \begin{bmatrix}0 & 0 & 0 \\{{- \omega_{ie}}\sin\; L} & 0 & 0 \\{\omega_{ie}\cos\; L} & 0 & 0\end{bmatrix}} + \ \begin{bmatrix}0 & 0 & {v_{N}/R_{h}^{2}} \\0 & 0 & {{- v_{E}}/R_{h}^{2}} \\{v_{E}\sec^{2}{L/R_{h}}} & 0 & {{- v_{E}}\tan\;{L/R_{h}^{2}}}\end{bmatrix}} \right)}},\mspace{20mu}{M_{32} = \begin{bmatrix}0 & {1/R_{h}} & 0 \\{\sec\;{L/R_{h}}} & 0 & 0 \\0 & 0 & 1\end{bmatrix}},\mspace{20mu}{M_{33} = {\begin{bmatrix}0 & 0 & {{- v_{N}}/R_{h}^{2}} \\{v_{E}\sec\; L\;\tan\;{L/R_{h}}} & 0 & {{- v_{E}}\sec\;{L/R_{h}^{2}}} \\0 & 0 & 0\end{bmatrix}\mspace{14mu}{and}}}}$$\mspace{20mu}{C_{b}^{n} = \begin{bmatrix}T_{11} & T_{12} & T_{13} \\T_{21} & T_{22} & T_{23} \\T_{31} & T_{32} & T_{33}\end{bmatrix}}$ is an attitude matrix of a sub-inertial navigation, T₁₁,T₁₂, T₁₃, T₂₁, T₂₂, T₂₃, T₃₁, T₃₂ and T₃₃ are elements of the attitudematrix, ε_(w) ^(b)=[ε_(wx) ^(b) ε_(wy) ^(b) ε_(wz) ^(b)]^(T) is aGaussian white noise vector measured by the gyro, ε_(wx) ^(b), ε_(wy)^(b) and ε_(wz) ^(b) are x, y, z axis gyro measurement Gaussian whitenoises respectively, ∇_(w) ^(b)=[∇_(wx) ^(b) ∇_(wy) ^(b) ∇_(wz)^(b)]^(T) is a white Gaussian vector of accelerometer measurement, and∇_(wx) ^(b), ∇_(wy) ^(b) and ∇_(wz) ^(b) are white Gaussian of x, y andz axial acceleration measurement.
 3. The method for the initialalignment of the radar assisted airborne strapdown inertial navigationsystem according to claim 2, wherein step 4 specifically includes: ameasurement Z=[R β α]^(T) includes the slant distance R, an azimuthangle β and a pitch angle α; where ${Z = {\begin{bmatrix}R \\\beta \\\alpha\end{bmatrix} = \begin{bmatrix}\sqrt{\left( {dx}^{n} \right)^{2} + \left( {dy}^{n} \right)^{2} + \left( {dz}^{n} \right)^{2}} \\{{arc}\;\tan\frac{{dz}^{n}}{\sqrt{\left( {dx}^{n} \right)^{2} + \left( {dy}^{n} \right)^{2}}}} \\{{arc}\;\tan\frac{{dx}^{n}}{{dy}^{n}}}\end{bmatrix}}},{\begin{bmatrix}{dx}^{n} & {dy}^{n} & {dz}^{n}\end{bmatrix}^{T} = {C_{p_{o}}^{n}\begin{bmatrix}{dx}^{e} & {dy}^{e} & {dz}^{e}\end{bmatrix}}^{T}}$ are relative position vectors of the targetaircraft and the tracking radar in the navigation coordinate system,C_(p) _(o) ^(n) is a coordinate conversion matrix between an earth'srectangular coordinate system and the navigation coordinate system, acoordinate of the earth's rectangular coordinate system is${\begin{bmatrix}{dx^{e}} \\{dy^{e}} \\{dz^{e}}\end{bmatrix} = {\begin{bmatrix}{\left( {R_{e} + h_{p}} \right)\cos\;\left( L_{p} \right)\cos\;\left( \lambda_{p} \right)} \\{\left( {R_{e} + h_{p}} \right)\cos\;\left( L_{p} \right)\sin\;\left( \lambda_{p} \right)} \\{\left( {R_{e} + h_{p}} \right)\sin\;\left( L_{p} \right)}\end{bmatrix} - \begin{bmatrix}x_{p_{o}}^{e} \\y_{p_{o}}^{e} \\z_{p_{o}}^{e}\end{bmatrix}}},$ e represents the Earth's rectangular coordinatesystem, L_(p)=L_(p) ^(s)−δL is a true latitude, λ_(p)=λ_(p) ^(s)−δλ is atrue longitude, h_(p)=h_(p) ^(s)−δh is a true altitude, and L_(p) ^(s),λ_(p) ^(s) and h_(p) ^(s) are positions resolved by the inertialnavigation system, then a measurement equation for the initial alignmentof the radar assisted airborne strapdown inertial navigation system is${Z = {{H\left( {{\delta L},{\delta\lambda},{\delta h}} \right)} + \begin{bmatrix}\omega_{R} \\\omega_{\beta} \\\omega_{\alpha}\end{bmatrix}}},$ where: ω_(R), ω_(α) and ω_(β) are white noises thatconform to a zero-mean Gaussian distribution, and an expression of anonlinear function H is obtained by substitution.